
#include "include.h"
#include "common/vector.h"
#include "common/utils.h"


#include "compile/debug_memoryleaks"


/**
* Constructor for a point with default coordinates.
*/
Vector::Vector() {
	//RS_DEBUG->print("Vector::Vector");
	set(0.0, 0.0, 0.0);
}



/**
* Constructor for a point with given coordinates.
*/
Vector::Vector(double vx, double vy, double vz) {
	//RS_DEBUG->print("Vector::Vector");
	set(vx, vy, vz);
}


/**
* Constructor for a point with given coordinates in an array
* or three doubles.
*/
//Vector::Vector(double v[]) {
//    set(v[0], v[1], v[2]);
//}


/**
* Constructor for a point with given valid flag.
*
* @param valid true: a valid vector with default coordinates is created.
*              false: an invalid vector is created
*/
Vector::Vector(bool valid) {
	//RS_DEBUG->print("Vector::Vector");
	set(0.0, 0.0, 0.0);
	this->valid = valid;
}


/**
* Destructor.
*/
Vector::~Vector() {
	//RS_DEBUG->print("Vector::~Vector");
}


/**
* Sets a new position for the vector.
*/
void Vector::set(double vx, double vy, double vz) {
	x = vx;
	y = vy;
	z = vz;
	valid = true;
}


/**
* Sets a new position for the vector in polar coordinates.
*/
void Vector::setPolar(double radius, double angle) {
	x = radius * cos(angle);
	y = radius * sin(angle);
	z = 0.0;
	valid = true;
}



/**
* @return The angle from zero to this vector (in rad).
*/
double Vector::angle() const {
	double ret = 0.0;
	double m = magnitude();

	if (m>1.0e-6) {
		double dp = dotP(*this, Vector(1.0, 0.0));
		//		RS_DEBUG->print("Vector::angle: dp/m: %f/%f", dp, m);
		if (dp/m>=1.0) {
			ret = 0.0;
		}
		else if (dp/m<-1.0) {
			ret = PI;
		}
		else {
			ret = acos( dp / m);
		}
		if (y<0.0) {
			ret = 2*PI - ret;
		}
	}
	return ret;
}



/**
* @return The angle from this and the given coordinate (in rad).
*/
double Vector::angleTo(const Vector& v) const {
	if (!valid || !v.valid) {
		return 0.0;
	}
	else {
		return (v-(*this)).angle();
	}
}



/**
* @return Magnitude (length) of the vector.
*/
double Vector::magnitude() const {
	double ret = 0.0;
	// Note that the z coordinate is also needed for 2d
	//   (due to definition of crossP())
	if (!valid) {
		ret = 0.0;
	}
	else {
		ret = sqrt(::pow(x,2) + ::pow(y,2) + ::pow(z,2));
	}

	return ret;
}


/**
*
*/
Vector Vector::lerp(const Vector& v, double t) const {
	return Vector(x+(v.x-x)*t, y+(v.y-y)*t);
}


/**
* @return The distance between this and the given coordinate.
*/
double Vector::distanceTo(const Vector& v) const {
	if (!valid || !v.valid) {
		return MAXDOUBLE;
	}
	else {
		return (*this-v).magnitude();
	}
}



/**
* @return true is this vector is within the given range.
*/
bool Vector::isInWindow(const Vector& firstCorner,
						const Vector& secondCorner) {

							double minX = std::min(firstCorner.x, secondCorner.x);
							double maxX = std::max(firstCorner.x, secondCorner.x);
							double minY = std::min(firstCorner.y, secondCorner.y);
							double maxY = std::max(firstCorner.y, secondCorner.y);

							return (x>=minX && x<=maxX && y>=minY && y<=maxY);
}



/**
* Moves this vector by the given offset. Equal to the operator +=.
*/
Vector Vector::move(Vector offset) {
	*this+=offset;
	return *this;
}



/**
* Rotates this vector around 0/0 by the given angle.
*/
Vector Vector::rotate(double ang) {
	//  RS_DEBUG->print("Vector::rotate: angle: %f", ang);

	double r = magnitude();

	//	RS_DEBUG->print("Vector::rotate: r: %f", r);

	double a = angle() + ang;

	//	RS_DEBUG->print("Vector::rotate: a: %f", a);

	x = cos(a) * r;
	y = sin(a) * r;

	//	RS_DEBUG->print("Vector::rotate: x/y: %f/%f", x, y);

	return *this;
}


/**
* Rotates this vector around the given center by the given angle.
*/
Vector Vector::rotate(Vector center, double ang) {
	*this = center + (*this-center).rotate(ang);
	return *this;
}


/**
* Scales this vector by the given factors with 0/0 as center.
*/
Vector Vector::scale(Vector factor) {
	x *= factor.x;
	y *= factor.y;
	return *this;
}



/**
* Scales this vector by the given factors with the given center.
*/
Vector Vector::scale(Vector center, Vector factor) {
	*this = center + (*this-center).scale(factor);
	return *this;
}



/**
* Mirrors this vector at the given axis.
*/
Vector Vector::mirror(Vector axisPoint1, Vector axisPoint2) {
	/*
	RS_ConstructionLine axis(NULL,
	RS_ConstructionLineData(axisPoint1, axisPoint2));

	Vector xp = axis.getNearestPointOnEntity(*this);
	xp = xp - (*this);
	(*this) += (xp*2);
	*/

	double phi1 = axisPoint1.angleTo(*this);
	double phi2 = axisPoint1.angleTo(axisPoint2) - phi1;
	double r1 = axisPoint1.distanceTo(*this);
	double r2 = axisPoint2.distanceTo(*this);

	if (r1<1.0e-6 || r2<1.0e-6) {
		// point touches one axis point
	}
	else {
		setPolar(r1, phi1 + 2*phi2);
		(*this) += axisPoint1;
	}

	return *this;
}



/**
* Streams the vector components to stdout. e.g.: "1/4/0"
*/
std::ostream& operator << (std::ostream& os, const Vector& v) {
	if(v.valid) {
		os << v.x << "/" << v.y << "/" << v.z;
	} else {
		os << "invalid vector";
	}
	return os;
}



/**
* binary + operator.
*/
Vector Vector::operator + (const Vector& v) const {
	return Vector(x + v.x, y + v.y, z + v.z);
}



/**
* binary - operator.
*/
Vector Vector::operator - (const Vector& v) const {
	return Vector(x - v.x, y - v.y, z - v.z);
}


/**
* binary * operator.
*/
Vector Vector::operator * (double s) const {
	return Vector(x * s, y * s, z * s);
}



/**
* binary / operator.
*/
Vector Vector::operator / (double s) const {
	return Vector(x / s, y / s, z / s);
}



/**
* unary - operator.
*/
Vector Vector::operator - () const {
	return Vector(-x, -y, -z);
}



/**
* Scalarproduct (dot product).
*/
double Vector::dotP(const Vector& v1, const Vector& v2) {
	return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}



/**
* += operator. Assert: both vectors must be valid.
*/
void Vector::operator += (const Vector& v) {
	x += v.x;
	y += v.y;
	z += v.z;
}


/**
* -= operator
*/
void Vector::operator -= (const Vector& v) {
	x -= v.x;
	y -= v.y;
	z -= v.z;
}



/**
* *= operator
*/
void Vector::operator *= (double s) {
	x *= s;
	y *= s;
	z *= s;
}



/**
* == operator
*/
bool Vector::operator == (const Vector& v) const {
	return (x==v.x && y==v.y && z==v.z && valid==v.valid);
}



/**
* @return A vector with the minimum components from the vectors v1 and v2.
* These might be mixed components from both vectors.
*/
Vector Vector::minimum (const Vector& v1, const Vector& v2) {
	return Vector (std::min(v1.x, v2.x),
		std::min(v1.y, v2.y),
		std::min(v1.z, v2.z));
}



/**
* @return A vector with the maximum values from the vectors v1 and v2
*/
Vector Vector::maximum (const Vector& v1, const Vector& v2) {
	return Vector (std::max(v1.x, v2.x),
		std::max(v1.y, v2.y),
		std::max(v1.z, v2.z));
}



/**
* @return Cross product of two vectors.
*/
Vector Vector::crossP(const Vector& v1, const Vector& v2) {
	return Vector(v1.y*v2.z - v1.z*v2.y,
		v1.z*v2.x - v1.x*v2.z,
		v1.x*v2.y - v1.y*v2.x);
}



